#include "CandidateFilter.h"
#include <string.h>

int rank_index[200];
long rank_value[200];
int cal_center[200][2];
int src_center[200][2];
#define _STA_V_COUNT_ (200)
#define _STA_H_COUNT_ (200)
#define _STA_INC_RATE (2)
#define _STA_DEC_RATE (2)
int sta_vbin[_STA_V_COUNT_];
int sta_hbin[_STA_H_COUNT_];

int candi_centerRanker(int src[][5], int count,int src_center[][2], int *src_flag, int dst[][5], int indst, int center[][2], int centercount, int lowbound, int upbound) {
	int i,j,tempi;
	long t1,t2,templ;
	int index;
	if(indst>=lowbound) return indst;
	memset(rank_value,1<<31,200);
	memset(rank_index,0,200);
	for(i=0;i<centercount;++i) {
		for(j=0;j<count;++j) {
			rank_index[j]=j;
			t1=src_center[j][0]-center[i][0];
			t2=src_center[j][1]-center[i][1];
			if(t1*t1+t2*t2<rank_value[j]) rank_value[j]=t1*t1+t2*t2;
		}
	}
	for(i=0;i<count;++i) {
		for(j=0;j<count-i;++j) {
			if(rank_value[j]>rank_value[j+1]){
				templ=rank_value[j];
				rank_value[j]=rank_value[j+1];
				rank_value[j+1]=templ;
				tempi=rank_index[j];
				rank_index[j]=rank_index[j+1];
				rank_index[j+1]=tempi;
			}
		}
	}
	for(i=0;i<count;++i) {
		index=rank_index[i];
		if(src_flag[index]==1) continue;
		dst[indst][0]=src[index][0];
		dst[indst][1]=src[index][1];
		dst[indst][2]=src[index][2];
		dst[indst][3]=src[index][3];
		dst[indst][4]=src[index][4];
/*		dst[0][indst]=src[0][index];
		dst[1][indst]=src[1][index];
		dst[2][indst]=src[2][index];
		dst[3][indst]=src[3][index];
		dst[4][indst]=src[4][index];*/
		indst++;
		if(indst>=upbound) break;
	}
	return indst;
}

int candi_statisticCenter(int src[][5], int src_center[][2], int count, int center[][2], int img_width, int img_height) {
	int vrate;
	int hrate;
	int tak;
	int i,j;
	int flag;
	int centercount=0;
	int maxv,max;
	long sumx=0,sumy=0;
	hrate=img_width/_STA_V_COUNT_;
	vrate=img_height/_STA_H_COUNT_;
	for(i=0;i<count;++i) {
		src_center[i][0]=(src[i][1]+src[i][0])/2;
		src_center[i][1]=(src[i][3]+src[i][2])/2;
		sumx+=src_center[i][0];
		sumy+=src_center[i][1];
		for(j=src[i][0]/vrate;j<=src[i][1]/vrate&&j<_STA_V_COUNT_;++i) sta_vbin[j]++;
		for(j=src[i][2]/hrate;j<=src[i][3]/hrate&&j<_STA_H_COUNT_;++j) sta_hbin[j]++;
	}
	/*sumx=sumx/count;
	sumy=sumy/count;
	center[0][0]=sumx;
	center[0][1]=sumy;
	return 1;*/
	flag=1;
	tak=0;
	while(flag==1) {
		flag=0;
		for(i=tak;i<_STA_V_COUNT_-tak;++i) {
			if(sta_vbin[i]/_STA_INC_RATE==0) {sta_vbin[i]=0;continue;}
			if(i>0&&i<_STA_V_COUNT_-1) {
				if(sta_vbin[i]<=sta_vbin[i-1]&&sta_vbin[i]<=sta_vbin[i+1]&&sta_vbin[i]>0) {sta_vbin[i-1]+=sta_vbin[i]/_STA_INC_RATE/2;sta_vbin[i+1]+=sta_vbin[i]/_STA_INC_RATE/2;sta_vbin[i]=sta_vbin[i]/_STA_DEC_RATE;flag=1;}
				if(sta_vbin[i]<=sta_vbin[i-1]&&sta_vbin[i]>sta_vbin[i+1]&&sta_vbin[i]>0) {sta_vbin[i-1]+=sta_vbin[i]/_STA_INC_RATE;sta_vbin[i]=sta_vbin[i]/_STA_DEC_RATE;flag=1;}
				if(sta_vbin[i]>sta_vbin[i-1]&&sta_vbin[i]<=sta_vbin[i+1]&&sta_vbin[i]>0) {sta_vbin[i+1]+=sta_vbin[i]/_STA_INC_RATE;sta_vbin[i]=sta_vbin[i]/_STA_DEC_RATE;flag=1;}
			}
			else {
				if(i==0)
					if(sta_vbin[i]<=sta_vbin[i+1]&&sta_vbin[i]>0) {sta_vbin[i+1]+=sta_vbin[i]/_STA_INC_RATE;sta_vbin[i]=sta_vbin[i]/_STA_DEC_RATE;flag=1;}
				if(i==_STA_V_COUNT_-1)
					if(sta_vbin[i]<=sta_vbin[i-1]&&sta_vbin[i]>0) {sta_vbin[i-1]+=sta_vbin[i]/_STA_INC_RATE;sta_vbin[i]=sta_vbin[i]/_STA_DEC_RATE;flag=1;}			
			}
		}
		tak=_STA_V_COUNT_-tak;
	}
	flag=1;
	tak=0;
	while(flag==1) {
		flag=0;
		for(i=tak;i<_STA_H_COUNT_-tak;++i) {
			if(sta_hbin[i]/_STA_INC_RATE==0) {sta_hbin[i]=0;continue;}
			if(i>0&&i<_STA_H_COUNT_-1) {
				if(sta_hbin[i]<=sta_hbin[i-1]&&sta_hbin[i]<=sta_hbin[i+1]&&sta_hbin[i]>0) {sta_hbin[i-1]+=sta_hbin[i]/_STA_INC_RATE/2;sta_hbin[i+1]+=sta_hbin[i]/_STA_INC_RATE/2;sta_hbin[i]=sta_hbin[i]/_STA_DEC_RATE;flag=1;}
				if(sta_hbin[i]<=sta_hbin[i-1]&&sta_hbin[i]>sta_hbin[i+1]&&sta_hbin[i]>0) {sta_hbin[i-1]+=sta_hbin[i]/_STA_INC_RATE;sta_hbin[i]=sta_hbin[i]/_STA_DEC_RATE;flag=1;}
				if(sta_hbin[i]>sta_hbin[i-1]&&sta_hbin[i]<=sta_hbin[i+1]&&sta_hbin[i]>0) {sta_hbin[i+1]+=sta_hbin[i]/_STA_INC_RATE;sta_hbin[i]=sta_hbin[i]/_STA_DEC_RATE;flag=1;}
			}
			else {
				if(i==0)
					if(sta_hbin[i]<=sta_hbin[i+1]&&sta_hbin[i]>0) {sta_hbin[i+1]+=sta_hbin[i]/_STA_INC_RATE;sta_hbin[i]=sta_hbin[i]/_STA_DEC_RATE;flag=1;}
				if(i==_STA_H_COUNT_-1)
					if(sta_hbin[i]<=sta_hbin[i-1]&&sta_hbin[i]>0) {sta_hbin[i-1]+=sta_hbin[i]/_STA_INC_RATE;sta_hbin[i]=sta_hbin[i]/_STA_DEC_RATE;flag=1;}
			}
		}
		tak=_STA_H_COUNT_-tak;
	}
	maxv=0;
	for(i=0;i<_STA_V_COUNT_;++i)
		if(sta_vbin[i]>maxv) {
			maxv=sta_vbin[i];
			max=i;
		}
	center[0][0]=(max*vrate+max*vrate+vrate)/2;
	maxv=0;
	for(i=0;i<_STA_H_COUNT_;++i)
		if(sta_hbin[i]>maxv) {
			maxv=sta_hbin[i];
			max=i;
		}
	center[0][1]=(max*hrate+max*hrate+hrate)/2;
	return 1;
}

int candi_candidateFilter(int src[][5], int *src_flag, int count, int dst[][5], int indst, int lowbound, int upbound, int img_width, int img_height) {
	int centercount;
	centercount=candi_statisticCenter(src,src_center,count,cal_center,img_width,img_height);
	return candi_centerRanker(src,count,src_center,src_flag,dst,indst,cal_center,centercount,lowbound,upbound);
}
